import sensor, image, time,math
from pyb import Pin
#from pid import PID
from pyb import Servo
from pyb import LED


#红追绿，由于红色激光照到黑胶布上，红色激光无法识别。而绿色的可以，
#因此我们使用绿色作为移动目标，红色作为跟踪。

#本舵机使用红色激光笔，去追踪绿设激光点。
#舵机结构红色激光笔与镜头一体，同时运动

#红色中心点初始位置为 170，110，这个根据自己激光笔和摄像头固定的位置测量得到，
#有可能存在误差
red_x=170
red_y=110


pan_servo=Servo(1)
tilt_servo=Servo(2)

#p_in0 = Pin('P0', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻，复位用
p_in1 = Pin('P1', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻，停止光标移动
p_in2 = Pin('P2', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻，重启光标移动
#如果单纯实现追踪，可忽略以上设置。
pan_servo.calibration(500,2500,500)
tilt_servo.calibration(500,2500,500)
time.sleep_ms(300)

gray_threshold = (0, 191)#灰度阈值根据实际情况可微调，排除最亮的点即可，后续程序有取反
green_threshold  = (100, 78, -32, 91, -60, 99)#此处LAB阈值红绿通用，不改程序名称了
sensor.reset() # Initialize the camera sensor.
#sensor.set_pixformat(sensor.RGB565) # use RGB565.
sensor.set_pixformat(sensor.GRAYSCALE) # use GRAYSCALE.
sensor.set_framesize(sensor.QVGA) # use QVGA for speed.
sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_whitebal(False) # turn this off.
clock = time.clock() # Tracks FPS.


while(True):

    # 但由于安装不够稳固，每次安装会有少许变化，需要在程序中进行修正。red_x,red_y
    # 跟踪过程实际上是用修正后的红色坐标去追踪绿色目标

    ss=[[0,0],[0,0],[0,0]] #存储识别到的亮点坐标，实际上只需要两个，防止溢出多开一个空间
    min_size=400
    for i in range (1000):
        if p_in1.value()==0:  #暂停mov_pwm
            while(True):
                if p_in2.value()==0:  #恢复mov_pwm
                    break
        j=0
        r=[900,900]  #初始化两个亮点到标准绿点的距离为一个比较大的值
        img = sensor.snapshot()
        #blobs = img.find_blobs([green_threshold],x_stride=1,y_stride=1)
        blobs = img.find_blobs([gray_threshold],x_stride=1,y_stride=1,invert=True)
        if i > 10 :
            for blob in blobs:
                #尽量过滤掉一些杂散的blobs
                if (blob[2]*blob[3] < min_size) and (blob[2]>2) and  (blob[3]>2) and (blob[2]<2*blob[3]) and (blob[3]<2*blob[2]):
#确保色块较小且较方（圆）
                    yuandian = blob
                    img.draw_rectangle(yuandian.rect(),color = (0,0,0)) # rect
                    img.draw_cross(yuandian.cx(), yuandian.cy()) # cx, cyj v
                    print(yuandian.cx(), yuandian.cy(),blob[2],blob[3])

                    if j>1:  #蓝色LED点亮，表示搜索到3个以上的目标，设备不能正常跟踪
                        LED(3).on()   #点亮蓝灯
                        time.sleep_ms(500)
                        #LED(3).off()
                        ss=[[0,0],[0,0],[0,0]]
                        break
                    else:    #逐个记录识别到的激光点
                        ss[j][0]=yuandian.cx()
                        ss[j][1]=yuandian.cy()
                    j=j+1

            if j==2: #只发现两个亮点
                LED(3).off()  #关闭蓝灯
                #计算两个亮点到初始红点的距离，谁近则谁就是追踪点，远的就是目标点
                r[0]=math.sqrt((ss[0][0]-red_x)*(ss[0][0]-red_x) + (ss[0][1]-red_y)*(ss[0][1]-red_y))
                r[1]=math.sqrt((ss[1][0]-red_x)*(ss[1][0]-red_x) + (ss[1][1]-red_y)*(ss[1][1]-red_y))

                #计算两个亮点之间的距离
                rr =math.sqrt((ss[0][0]-ss[1][0])*(ss[0][0]-ss[1][0]) + (ss[0][1]-ss[1][1])*(ss[0][1]-ss[1][1]))
                if rr>20:  #当两个亮点距离比较大的时候，动态的修正red_x，和red_y
#即将距离近的点坐标幅值给初始红坐标。
                    if (r[0]<r[1]) and (r[0]<30) :
                        red_x = ss[0][0]
                        red_y = ss[0][1]
                    if (r[1]<r[0]) and (r[1]<30) :
                        red_x = ss[1][0]
                        red_y = ss[1][1]

                if rr<6:  #如果两点之间的距离小于6个像素，则认为已经跟踪上了
                    LED(2).on()   #点亮绿灯
                else:
                    LED(2).off()   #熄灭绿灯
                    now_x=pan_servo.pulse_width()
                    now_y=tilt_servo.pulse_width()
                    dx = ss[0][0]-ss[1][0]
                    dy = ss[0][1]-ss[1][1]
                    if r[0]>r[1]:  # 再次根据远近判断红绿点，并确定距离方向。因为两点初始距离若小于等于20，则程序没有对预设的红点坐标进行动态校正。
                        dx=-dx
                        dy=-dy

                    if p_in1.value()==0:  #暂停mov_pwm
                        while(True):
                            if p_in2.value()==0:  #恢复mov_pwm
                                break

                    if dx>5:
                        pan_servo.pulse_width(now_x+10)  #最小步进是10，也许不同舵机有出入，可自行调整尝试。
                    if dx<-5:
                        pan_servo.pulse_width(now_x-10)
                    if dy>5:
                        tilt_servo.pulse_width(now_y+10)
                    if dy<-5:
                        tilt_servo.pulse_width(now_y-10)

                    if math.sqrt(dx*dx + dy*dy)>30 :
                        time.sleep_ms(10)
                    else:
                        time.sleep_ms(50)

            if j==1 and r[0]>6: #如果只发现一个亮点，则用标准红点red_x,red_y,去追踪这个点
                now_x=pan_servo.pulse_width()
                now_y=tilt_servo.pulse_width()
                dx = red_x-ss[0][0]
                dy = red_y-ss[0][1]

                if p_in1.value()==0:  #暂停mov_pwm
                    while(True):
                        if p_in2.value()==0:  #恢复mov_pwm
                            break

                if dx>5:
                    pan_servo.pulse_width(now_x+10)  #最小步进是10
                if dx<-5:
                    pan_servo.pulse_width(now_x-10)
                if dy>5:
                    tilt_servo.pulse_width(now_y+10)
                if dy<-5:
                    tilt_servo.pulse_width(now_y-10)

                if math.sqrt(dx*dx + dy*dy)>30 :  #根据距离不同设置休眠时间
                    time.sleep_ms(10)
                else:
                    time.sleep_ms(50)

    print("1111111111111111111111111111111111")
